#include "Vector2.h"

namespace Core
{
	rtVector2 rtVector2::vec2_origin( 0.0f, 0.0f );

	inline rtVector2::rtVector2(void) 
	{
	}

	inline rtVector2::rtVector2(const float x, const float y) 
	{
		this->x = x;
		this->y = y;
	}

	inline void rtVector2::set(const float x, const float y) 
	{
		this->x = x;
		this->y = y;
	}

	inline void rtVector2::zero(void) 
	{
		x = y = 0.0f;
	}

	inline bool rtVector2::compare(const rtVector2 &a) const 
	{
		return ((x == a.x) && (y == a.y));
	}

	inline bool rtVector2::compare(const rtVector2 &a, const float epsilon) const 
	{
		if(rtMath::Fabs(x - a.x) > epsilon) 
		{
			return false;
		}

		if(rtMath::Fabs(y - a.y) > epsilon)
		{
			return false;
		}

		return true;
	}

	inline bool rtVector2::operator == (const rtVector2 &a) const 
	{
		return compare(a);
	}

	inline bool rtVector2::operator != (const rtVector2 &a) const 
	{
		return !compare(a);
	}

	inline float rtVector2::operator [] (int index) const 
	{
		return (&x)[index];
	}

	inline float& rtVector2::operator[]( int index ) 
	{
		return (&x)[index];
	}

	inline float rtVector2::length(void) const 
	{
		return (float)rtMath::Sqrt(x * x + y * y);
	}

	inline float rtVector2::lengthFast(void) const {
		float sqrLength;

		sqrLength = x * x + y * y;
		return sqrLength * rtMath::RSqrt(sqrLength);
	}

	inline float rtVector2::lengthSqr(void) const 
	{
		return (x * x + y * y);
	}

	inline float rtVector2::normalize(void) 
	{
		float sqrLength, invLength;

		sqrLength = x * x + y * y;
		invLength = rtMath::InvSqrt(sqrLength);
		x *= invLength;
		y *= invLength;
		return invLength * sqrLength;
	}

	inline float rtVector2::normalizeFast(void) 
	{
		float lengthSqr, invLength;

		lengthSqr = x * x + y * y;
		invLength = rtMath::RSqrt(lengthSqr);
		x *= invLength;
		y *= invLength;
		return invLength * lengthSqr;
	}

	inline rtVector2& rtVector2::truncate(float length) 
	{
		float length2;
		float ilength;

		if (!length) 
		{
			zero();
		}
		else 
		{
			length2 = lengthSqr();
			if (length2 > length * length) 
			{
				ilength = length * rtMath::InvSqrt( length2 );
				x *= ilength;
				y *= ilength;
			}
		}

		return *this;
	}

	inline void rtVector2::clamp(const rtVector2 &min, const rtVector2 &max) 
	{
		if (x < min.x) 
		{
			x = min.x;
		} 
		else if (x > max.x) 
		{
			x = max.x;
		}
		if (y < min.y) 
		{
			y = min.y;
		} 
		else if (y > max.y) 
		{
			y = max.y;
		}
	}

	inline void rtVector2::snap(void) 
	{
		x = floor(x + 0.5f);
		y = floor(y + 0.5f);
	}

	inline void rtVector2::snapInt(void) 
	{
		x = float(int(x));
		y = float(int(y));
	}

	inline rtVector2 rtVector2::operator - () const 
	{
		return rtVector2(-x, -y);
	}

	inline rtVector2 rtVector2::operator - (const rtVector2 &a) const 
	{
		return rtVector2(x - a.x, y - a.y);
	}

	inline float rtVector2::operator * (const rtVector2 &a) const 
	{
		return x * a.x + y * a.y;
	}

	inline rtVector2 rtVector2::operator * (const float a) const 
	{
		return rtVector2(x * a, y * a);
	}

	inline rtVector2 rtVector2::operator / (const float a) const 
	{
		float inva = 1.0f / a;
		return rtVector2(x * inva, y * inva);
	}

	inline rtVector2 operator * (const float a, const rtVector2 b) 
	{
		return rtVector2(b.x * a, b.y * a);
	}

	inline rtVector2 rtVector2::operator + (const rtVector2 &a) const 
	{
		return rtVector2( x + a.x, y + a.y );
	}

	inline rtVector2 &rtVector2::operator += (const rtVector2 &a) 
	{
		x += a.x;
		y += a.y;

		return *this;
	}

	inline rtVector2& rtVector2::operator /= (const rtVector2 &a) 
	{
		x /= a.x;
		y /= a.y;

		return *this;
	}

	inline rtVector2& rtVector2::operator /= (const float a) 
	{
		float inva = 1.0f / a;
		x *= inva;
		y *= inva;

		return *this;
	}

	inline rtVector2& rtVector2::operator -= (const rtVector2 &a) 
	{
		x -= a.x;
		y -= a.y;

		return *this;
	}

	inline rtVector2& rtVector2::operator *= (const float a) 
	{
		x *= a;
		y *= a;

		return *this;
	}

	inline int rtVector2::getDimension(void) const 
	{
		return 2;
	}

	inline const float* rtVector2::toFloatPtr(void) const 
	{
		return &x;
	}

	inline float* rtVector2::toFloatPtr(void) 
	{
		return &x;
	}
	void rtVector2::lerp(const rtVector2 &v1, const rtVector2 &v2, const float l) 
	{
		if (l <= 0.0f) 
		{
			(*this) = v1;
		} 
		else if (l >= 1.0f) 
		{
			(*this) = v2;
		} 
		else 
		{
			(*this) = v1 + l * (v2 - v1);
		}
	}

}

